#!coding:utf-8
import numpy as np
from .config import Command
from pi_driver import MyJoy


class JoystickInterface:
    def __init__(
        self, config
    ):
        self.config = config
        self.previous_gait_toggle = 0
        self.previous_activate_toggle = 0
        self.joystick = MyJoy(callback=None)

    def get_command(self, state, do_print=False):
        try:
            msg = self.get_joystick()
            command = Command()
            
            # 静止/运动 步态切换
            gait_toggle = msg["R1"]
            command.trot_event = (
                gait_toggle == 1 and self.previous_gait_toggle == 0)

            # 未激活/激活 状态切换
            activate_toggle = msg["L1"]
            command.activate_event = (
                activate_toggle == 1 and self.previous_activate_toggle == 0)

            # 更新切换状态
            self.previous_gait_toggle = gait_toggle
            self.previous_activate_toggle = activate_toggle

            # 速度由左摇杆控制
            x_vel = msg["ly"] * self.config.max_x_velocity
            y_vel = msg["lx"] * -self.config.max_y_velocity
            command.horizontal_velocity = np.array([x_vel, y_vel])

            command.yaw = msg["rx"] * -self.config.max_yaw

            command.pitch = msg["ry"] * self.config.max_pitch

            height_movement = msg["dpady"]
            command.height = state.height - self.config.dt * \
                self.config.z_speed * height_movement

            roll_movement = - msg["dpadx"]
            command.roll = state.roll + self.config.dt * \
                self.config.roll_speed * roll_movement

            return command

        except Exception as e:
            if do_print:
                print(e)
            return Command()

    # 手柄摇杆、按键 到 状态信息的转换
    def get_joystick(self):
        buttons = self.joystick.buttons
        axes = self.joystick.axes
        left_y = -axes[1]/100.0
        right_y = -axes[4]/100.0
        right_x = axes[3]/100.0
        left_x = axes[0]/100.0

        L2 = axes[2]/100.0
        R2 = axes[5]/100.0

        R1 = buttons[5]
        L1 = buttons[4]

        square = buttons[2]
        x = buttons[0]
        circle = buttons[1]
        triangle = buttons[3]

        dpadx = axes[6]/100.0
        dpady = -axes[7]/100.0
        if len(buttons) >= 17:
            dpadx = buttons[15]-buttons[16]
            dpady = buttons[13]-buttons[14]


        msg = {
            "ly": left_y,
            "lx": left_x,
            "rx": right_x,
            "ry": right_y,
            "L2": L2,
            "R2": R2,
            "R1": R1,
            "L1": L1,
            "dpady": dpady,
            "dpadx": dpadx,
            "x": x,
            "square": square,
            "circle": circle,
            "triangle": triangle,
        }
        return msg
